/*
 * 文件名：bsp_calibration.cpp
 * 功能描述：通过霍尔传感器检测磁场变化，完成对内臂和外臂的校准
 */
#include "bsp_calibration.h"
#include <Arduino.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
#include "MeanFilterLib.h"
/**
 *  * 空引脚: 250以内
 * 正常电平(1.65V): 1910
 * 3050
 * 900
 * N 从Hall背面穿到正面: 变小
 * N 从Hall正面穿到背面: 变大
 *
 * Arduino控制器读取一次模拟输入需要消耗100微秒的时间（0.0001秒）。
 * 控制器读取模拟输入的最大频率是每秒10，000次。
 *
 *    calb_arm(0);
    // 逆向动1.8°
//    calb_arm(1);

 * */

/** 四种驱动方式
block方式 移动步长, 读取霍尔  [选择, 最简单,独立]
Timer定时器方式 一直运动, 读取霍尔
TMC一直step运动
TMC固定角度 用库position
*/

// int dato_hall1 = 0;;
// int dato_hall2 = 0;;
uint hall_zero1 = 0;
uint hall_zero2 = 0;
uint hall_pole1 = 0;
uint hall_pole2 = 0;

MeanFilter<long> meanFilter_40(40);

int read_hall1(void)
{
    return analogRead(hall1);
}
int read_hall2(void)
{
    return analogRead(hall2);
}

/**
 *@brief 这个边界函数通过步数控制电机的移动。
 *@param steps 这个变量表示要前进的步骤数。
 *@param motor_d 表示要移动的引擎，选项1或2。
 *@param speed 表示转弯速度，如果该值减少，速度就会增加，如果该值增加，速度就会减少。
 */
void move(int steps, int motor_d, int Speed)
{
    if (motor_d == 1)
    {
        for (int i = 0; i < steps; i++)
        {
            digitalWrite(STEP_PIN, HIGH);
            delayMicroseconds(Speed);
            digitalWrite(STEP_PIN, LOW);
            delayMicroseconds(Speed);
        }
    }

    if (motor_d == 2)
    {
        for (int j = 0; j < steps; j++)
        {
            digitalWrite(STEP_PIN2, HIGH);
            delayMicroseconds(Speed);
            digitalWrite(STEP_PIN2, LOW);
            delayMicroseconds(Speed);
        }
    }
}

//// 移动特定度数, 度数是1.8的倍数
uint move_degree(uint degree_1_8, uint motor_d)
{
    int cnt = 0;
    int a_pulse_x = x_microstep * x_arm_ratio;
    int a_pulse_y = y_microstep * y_arm_ratio;
    int a_pulse;
    if (motor_d == 1)
    {
        a_pulse = a_pulse_x;
    }
    else
    {
        a_pulse = a_pulse_y;
    }
    float mean_l;
    while (cnt < degree_1_8)
    {
        move(a_pulse, motor_d, 1000);

        for (int i = 0; i < 40; i++)
        {
            if (motor_d == 1)
            {
                mean_l = meanFilter_40.AddValue(analogRead(hall1));
            }
            else
            {
                mean_l = meanFilter_40.AddValue(analogRead(hall2));
            }
        }
        cnt++;
    }
    return mean_l;
}

//// 获得标准电平
uint find_average_of_min_two(int motor_d)
{
    float degree[5];
    int i, j;
    float temp;
    float avg;

    for (i = 0; i < 5; i++)
    {
        degree[i] = move_degree(6, motor_d);
    }

    // Sort the array in ascending order
    for (i = 0; i < 5; ++i)
    {
        for (j = i + 1; j < 5; ++j)
        {
            if (degree[i] > degree[j])
            {
                temp = degree[i];
                degree[i] = degree[j];
                degree[j] = temp;
            }
        }
    }

    // print degree
    //    for (i = 0; i < 5; ++i) {
    //        Serial.printf("%d, %.0f\n", i, degree[i]);
    //    }
    //    Serial.printf("_______________\n");

    // Calculate average of first two elements which are the smallest
    avg = (degree[0] + degree[1]) / 2;

    return uint(avg);
}

//// 获得磁铁极性
/**
 *  转超过1圈获取极点, 根据极点比平均值大或小判断极性
 转到极值点会退出循环,停止旋转, return
 缺点: 阈值600是经验值
 * */
int get_pole(uint motor_d)
{
    int cnt = 0;
    int a_pulse_x = x_microstep * x_arm_ratio;
    int a_pulse_y = y_microstep * y_arm_ratio;
    int a_pulse;
    if (motor_d == 1)
    {
        a_pulse = a_pulse_x;
    }
    else
    {
        a_pulse = a_pulse_y;
    }
    float mean_l;
    int delta_threshold = 600; //// 实测HALL平行磁铁动有时候会有600的反方向误差

    int zero_l = 0;
    int hall_l = 0;
    if (motor_d == 1)
    {
        zero_l = hall_zero1;
        hall_l = hall1;
    }
    else if (motor_d == 2)
    {
        zero_l = hall_zero2;
        hall_l = hall2;
    }
    else
    {
        return -1;
    }
    int delta = 0;

    // 转220 pulse
    while (cnt < 200 + 20)
    {
        move(a_pulse, motor_d, 1000);

        for (int i = 0; i < 40; i++)
        {
            mean_l = meanFilter_40.AddValue(analogRead(hall_l));
        }

        delta = mean_l - zero_l;
        Serial.printf("motor_d:%d, delta:%d\n", motor_d, delta);

        if (delta > delta_threshold || delta < -delta_threshold)
        {
            break;
        }

        cnt++;
    }

    //    Serial.printf("Get delta:%d\n",delta);
    if (delta > delta_threshold)
    {
        return 1;
    }
    else if (delta < -delta_threshold)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

//// 获得反相的电平
uint8_t get_reverse_level(uint8_t level)
{
    if (level == LOW)
    {
        return HIGH;
    }
    else
    {
        return LOW;
    }
}

typedef struct Result
{
    uint16_t hall_peak;
    uint16_t hall_peak_index;

    uint16_t hall_peak_90;

    uint16_t hall_peak_90_start;
    uint16_t hall_peak_90_start_index;

    uint16_t hall_peak_90_end;
    uint16_t hall_peak_90_end_index;

    uint16_t diff;
} Result;
#define HALL_VAL_LEN 200
int hall_val_arr[HALL_VAL_LEN] = {0};
Result fine_res;
Result calculate(int hall_pole, int hall_zero, int hall_val[])
{

    Result res;
    int hall_min = hall_zero;
    int hall_max = 0;

    int hall_peak_90 = 0;
    int hall_peak_90_start = -1;
    uint16_t hall_peak_90_start_index = 0;
    int hall_peak_90_end = -1;
    uint16_t hall_peak_90_end_index = 0;

    // 打印前5个值
    //    for (int i = 0; i < 5; i++) {
    //        Serial.printf("in calculate %d: %d\n", i, hall_val_arr[i]);
    //    }

    ////极值计算
    if (hall_pole == 0)
    { ////hall_pole==0 有最小值 hall_min
        Serial.printf("Pole is 0\n");
        // 获得hall_val[HALL_VAL_LEN]数组的最小值
        for (int i = 0; i < HALL_VAL_LEN; i++)
        {
            if (hall_val[i] < hall_min)
            {
                hall_min = hall_val[i];
                res.hall_peak_index = i;
            }
        }

        res.hall_peak = hall_min;

        hall_peak_90 = hall_zero + (int)((hall_min - hall_zero) * 0.9);
        res.hall_peak_90 = hall_peak_90;

        //// 寻找 90% 首尾
        for (int i = 0; i < HALL_VAL_LEN; i++)
        {
            if (hall_val[i] < hall_peak_90 && hall_peak_90_start == -1)
            { //// 小于即突破
                hall_peak_90_start = hall_val[i];
                hall_peak_90_start_index = i;
            }
            if (hall_val[i] > hall_peak_90 && hall_peak_90_start != -1 && hall_peak_90_end == -1)
            {
                hall_peak_90_end = hall_val[i - 1];
                hall_peak_90_end_index = i - 1;
            }
        }
    }
    else
    { ////hall_pole==1 有最大值 hall_max _____________________________________________
        Serial.printf("Pole is 1\n");

        // 获得hall_val[HALL_VAL_LEN]数组的最大值
        for (int i = 0; i < HALL_VAL_LEN; i++)
        {
            if (hall_val[i] > hall_max)
            {
                hall_max = hall_val[i];
                res.hall_peak_index = i;
            }
        }
        res.hall_peak = hall_max;

        hall_peak_90 = hall_zero + (int)((hall_max - hall_zero) * 0.9);
        res.hall_peak_90 = hall_peak_90;

        //// 寻找 90% 首尾
        for (int i = 0; i < HALL_VAL_LEN; i++)
        {
            if (hall_val[i] > hall_peak_90 && hall_peak_90_start == -1)
            { //// 大于即突破
                hall_peak_90_start = hall_val[i];
                hall_peak_90_start_index = i;
            }
            if (hall_val[i] < hall_peak_90 && hall_peak_90_start != -1 && hall_peak_90_end == -1)
            {
                hall_peak_90_end = hall_val[i - 1];
                hall_peak_90_end_index = i - 1;
            }
        }
    }

    res.diff = hall_peak_90_end_index - hall_peak_90_start_index;

    // 将res中的值都用Serial.printf打印出来
    Serial.printf("[%d] hall_peak:%d \n", res.hall_peak_index, res.hall_peak);
    Serial.printf("hall_peak_90:%d, [%d] hall_peak_90_start:%d, [%d] hall_peak_90_end:%d, diff:%d\n", hall_peak_90, hall_peak_90_start_index, hall_peak_90_start, hall_peak_90_end_index, hall_peak_90_end, res.diff);

    return res;
}

/**

 * */
int fine_tune(uint motor_d)
{
    if (motor_d == 1)
    {
        Serial.printf("Motor[1] ");
    }
    else
    {
        Serial.printf("Motor[2] ");
    }
    Serial.printf("finetune Start\n");
    float mean_l;

    int zero_l = 0;
    int pole_l = 0;
    int hall_pin_l = 0;
    uint8_t motor_pin_l = DIR_PIN;
    uint8_t motor_dir_l = x_current_dir_g;

    if (motor_d == 1)
    {
        zero_l = hall_zero1;
        pole_l = hall_pole1;
        hall_pin_l = hall1;
        motor_pin_l = DIR_PIN;
        motor_dir_l = x_current_dir_g;
    }
    else if (motor_d == 2)
    {
        zero_l = hall_zero2;
        pole_l = hall_pole2;
        hall_pin_l = hall2;
        motor_pin_l = DIR_PIN2;
        motor_dir_l = y_current_dir_g;
    }
    else
    {
        return -1;
    }
    int delta = 0;

    ////## 逆向转12°，直到脱离原点
    motor_dir_l = get_reverse_level(motor_dir_l);
    digitalWrite(motor_pin_l, motor_dir_l);
    move_degree(12, motor_d);

    /// 处理数组
    if (pole_l == 1)
    { ////有最大值,全部赋0
        for (int i = 0; i < HALL_VAL_LEN; i++)
        {
            hall_val_arr[i] = 0;
        }
    }
    else
    { ////有最小值,全部赋 中性零电平
        for (int i = 0; i < HALL_VAL_LEN; i++)
        {
            hall_val_arr[i] = zero_l;
        }
    }

    ////## 顺时针转20度，记录不超过200个值
    motor_dir_l = get_reverse_level(motor_dir_l);
    digitalWrite(motor_pin_l, motor_dir_l);

    int a_pulse_x = x_microstep * x_arm_ratio;
    int a_pulse_y = y_microstep * y_arm_ratio;
    int a_pulse;
    if (motor_d == 1)
    {
        a_pulse = a_pulse_x;
    }
    else
    {
        a_pulse = a_pulse_y;
    }

    uint pts_idx = 0;
    int cnt = 0;

    while (cnt < (22 * a_pulse))
    {
        move(1, motor_d, 2000);

        for (int i = 0; i < 40; i++)
        {
            mean_l = meanFilter_40.AddValue(analogRead(hall_pin_l));
        }

        if (pts_idx < HALL_VAL_LEN)
        {
            hall_val_arr[pts_idx] = mean_l;
        }
        else
        {
            hall_val_arr[(HALL_VAL_LEN - 1)] = mean_l;
        }
        pts_idx++;

        cnt++;
    }
    Serial.printf("pts_idx: %d\n", pts_idx);

    //// 计算极值
    fine_res = calculate(pole_l, zero_l, hall_val_arr);

    //// 再逆时针对齐
    motor_dir_l = get_reverse_level(motor_dir_l);
    digitalWrite(motor_pin_l, motor_dir_l);

    uint exit_idx = 0;
    int exit_cnt = 0;
    while (exit_cnt < (11 * a_pulse))
    {
        move(1, motor_d, 5000);

        for (int i = 0; i < 40; i++)
        {
            mean_l = meanFilter_40.AddValue(analogRead(hall_pin_l));
        }
        //        Serial.printf("%.0f\n", mean_l);
        exit_idx++;
        if (pole_l == 1)
        {
            if (mean_l > fine_res.hall_peak_90)
            {
                uint need_steps = (uint)(fine_res.diff / 2); //// 回转1半
                Serial.printf("Has Moved: %d, need move last diff steps: %d\n", exit_idx, need_steps);
                move(need_steps, motor_d, 5000);
                for (int i = 0; i < 40; i++)
                {
                    mean_l = meanFilter_40.AddValue(analogRead(hall_pin_l));
                }
                Serial.printf("Final Hall Value:%.0f\n", mean_l);
                break;
            }
        }
        else
        {
            if (mean_l < fine_res.hall_peak_90)
            {
                uint need_steps = (uint)(fine_res.diff / 2);
                Serial.printf("Has Moved: %d, need move last diff steps: %d\n", exit_idx, need_steps);
                move(need_steps, motor_d, 5000);
                for (int i = 0; i < 40; i++)
                {
                    mean_l = meanFilter_40.AddValue(analogRead(hall_pin_l));
                }
                Serial.printf("Final Hall Value:%.0f\n", mean_l);
                break;
            }
        }

        cnt++;
    }

    if (motor_d == 1)
    {
        Serial.printf("Motor[1] ");
    }
    else
    {
        Serial.printf("Motor[2] ");
    }
    Serial.printf("finetune Finish\n");

    return 0; ////必须要有return
}

/**
 * 总体思路:
 * 先对齐内臂,再对齐外臂
 * 每个臂先粗对齐,精对齐
 * */
int calb_arm(int origin_type)
{

    Serial.printf("\n\n");
    //// 对齐内臂 ___________________________________________________
    ////## 顺指针转, 取5个10°中最小2个区间的平均值,获得 zero 电平值
    hall_zero1 = find_average_of_min_two(1);
    Serial.printf("hall_zero1 :%d\n", hall_zero1);

    ////## Pole 转到gap>阈值 停
    hall_pole1 = get_pole(1);
    if (hall_pole1 >= 0)
    {
        Serial.printf("hall_pole1 :%d\n", hall_pole1);
    }
    else
    {
        Serial.printf("ERROR: check hall or magnet\n");
    }

    //// 精细对齐
    fine_tune(1);

    //// 对齐外臂 ___________________________________________________
    hall_zero2 = find_average_of_min_two(2);
    Serial.printf("hall_zero2 :%d\n", hall_zero2);

    hall_pole2 = get_pole(2);
    if (hall_pole2 >= 0)
    {
        Serial.printf("hall_pole2 :%d\n", hall_pole2);
    }
    else
    {
        Serial.printf("ERROR: check hall or magnet\n");
    }

    fine_tune(2);

    Serial.printf("Summary:\n");
    Serial.printf("hall_zero :%d, %d\n", hall_zero1, hall_zero2);
    Serial.printf("hall_pole :%d, %d\n", hall_pole1, hall_pole2);
    Serial.printf("Calibration Finished\n");

    if (origin_type == 0)
    { //// 回到 (0,0)点
        ;
    }
    else
    { //// 回到 边点, 外臂移动半圈
        move(0.5 * y_circle, 2, 1000);
    }
    return 0;
}

//_______________________________________________
//// hall 1曲线检测
void continuous_read1(long ms)
{

    // 持续10s
    unsigned long start_time = millis();
    unsigned long end_time = start_time + ms;
    while (millis() < end_time)
    {

        int dato_hall1;
        float mean;
        for (int i = 0; i < 40; i++)
        { // 实测耗时几乎没有影响
            dato_hall1 = analogRead(hall1);
            mean = meanFilter_40.AddValue(dato_hall1);
        }
        Serial.print(dato_hall1);
        Serial.print(",");
        // 按%.0f打印mean
        Serial.println(mean, 0);
    }
}

//// hall 2曲线检测
void continuous_read2(long ms)
{
    // 持续10s
    unsigned long start_time = millis();
    unsigned long end_time = start_time + ms;
    while (millis() < end_time)
    {

        int dato_hall2;
        float mean;
        for (int i = 0; i < 40; i++)
        { // 实测耗时几乎没有影响
            dato_hall2 = analogRead(hall2);
            mean = meanFilter_40.AddValue(dato_hall2);
        }
        //        Serial.printf("%d, %.0f\n", dato_hall2, mean);
        Serial.print(dato_hall2);
        Serial.print(",");
        // 按%.0f打印mean
        Serial.println(mean, 0);
    }
}

//// 步进电机1 运动中曲线检测
void run_circle_read_hall1(uint8_t num_of_turns)
{
    int a_pulse_x = x_microstep * x_arm_ratio;

    long cnt = 0;
    while (cnt < (200 * a_pulse_x * num_of_turns))
    {
        move(1, 1, 1000);
        int dato_hall1;
        float mean;
        for (int i = 0; i < 40; i++)
        { // 实测耗时几乎没有影响
            dato_hall1 = analogRead(hall1);
            mean = meanFilter_40.AddValue(dato_hall1);
        }
        Serial.print(dato_hall1);
        Serial.print(",");
        // 按%.0f打印mean
        Serial.println(mean, 0);

        cnt++;
    }
}

//// 步进电机2 运动中曲线检测
void run_circle_read_hall2(uint8_t num_of_turns)
{
    int a_pulse_y = x_microstep * x_arm_ratio;
    long cnt = 0;
    while (cnt < (200 * a_pulse_y * num_of_turns))
    {
        move(1, 2, 1000);
        int dato_hall2;
        float mean;
        for (int i = 0; i < 40; i++)
        { // 实测耗时几乎没有影响
            dato_hall2 = analogRead(hall2);
            mean = meanFilter_40.AddValue(dato_hall2);
        }
        Serial.print(dato_hall2);
        Serial.print(",");
        // 按%.0f打印mean
        Serial.println(mean, 0);
        cnt++;
    }
}
